#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "drivers/common.h"
#include "drivers/HTPB-driver.h"
#include "drivers/big_board.h"

#define leftB 2
#define rightB 1

/**********************************************************************\
*																																	    *
* PROGRAM: Running test program for Arm height         	      			  *
* VERSION: 2.0  																									    *
* PURPOSE: Controllable user program for arm height period.           *
* AUTHOR: Samuel Rosenstein                                             *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/


void _allignHeight(int height) {
  int current;
  current = HTPBreadADC(HTPB, 4, 10);

  height = max(height, MIN_HEIGHT_ARM);

  while (abs(height - current) > 10) {
    current = HTPBreadADC(HTPB, 4, 10);
    if ((current - height) > 0) {
      motor[f] = 15;
    }
    if ((current - height) < 0) {
      motor[f] = -9;
    }
    nxtDisplayTextLine(2, "current: %4d", current);
  }
  motor[f] = 0;
}

void allignHeight(int height) {
  int current;
  current = HTPBreadADC(HTPB, 4, 10);
  if ((current - height) > 0) {
    _allignHeight(height - 50);
    wait1Msec(100);
  }
  _allignHeight(height);
}


task main() {
  int height;
  int current;
  int speed;
  height = HTPBreadADC(HTPB, 4, 10);
  while (true){
    current = HTPBreadADC(HTPB, 4, 10);
    nMotorPIDSpeedCtrl[f] = mtrSpeedReg;
    wait10Msec(60);
    while (nNxtButtonPressed != 3) {
      nxtDisplayTextLine(3, "height: %4d", height);
      if (nNxtButtonPressed == leftB) {
        height = height + 10;
        wait10Msec(20);
      }
      if (nNxtButtonPressed == rightB) {
        height = height - 10;
        wait10Msec(20);
      }
      int current;
      current = HTPBreadADC(HTPB, 4, 10);
      nxtDisplayTextLine(2, "current: %4d", current);
      nxtDisplayTextLine(3, "height: %4d", height);
    }
    allignHeight(height);
    current = HTPBreadADC(HTPB, 4, 10);
    nxtDisplayTextLine(2, "current: %4d", current);
  }
}
